/**
 * @file dijkstral.cpp
 * @author 范文宇 (2643660853@qq.com)
 * @brief 
 * @version 1.0
 * @date 2022-10-21
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "dijkstral.h"
using namespace std;
using namespace cv;

Dijkstral::Dijkstral(int point_width, int point_height,
                     cv::Point2i startingPoint, cv::Point2i finishingPoint,
                     const std::vector<int> &vertex, const cv::Mat1d &edge)
{
    dist.reserve(point_width * point_height);
    path.reserve(point_width * point_height);
    for (int i = 0; i < point_width * point_height; i++)
    {
        dist.push_back(edge.at<double_t>(startingPoint.x + startingPoint.y * point_width, i));
        path.push_back(std::vector<int>());
        path[i].push_back(startingPoint.x + startingPoint.y * point_width);
        if (dist[i] != 1e7)
        {
            path[i].push_back(i);
        }
    }
}

int Dijkstral::minOfDist()
{
    int min_dex = -1;
    int min_dist = 1e7;
    for (int i = 0; i < dist.size(); i++)
    {
        if (dist[i] < min_dist && dist[i] != 0)
        {
            min_dex = i;
            min_dist = dist[i];
        }
    }
    return min_dex;
}

std::vector<int> &Dijkstral::getShortestPath(int point_width, int point_height,
                                             cv::Point2i startingPoint, cv::Point2i finishingPoint,
                                             const std::vector<int> &vertex, const cv::Mat1d &edge)
{
    int k = 0;
    for (int i = 1; i < point_width * point_height; i++)
    {
        k = minOfDist();
        if (k != -1)
        {
            for (int j = 0; j < point_width * point_height; j++)
            {
                if (dist[j] > dist[k] + edge.at<double_t>(k, j))
                {
                    dist[j] = dist[k] + edge.at<double_t>(k, j);
                    path[j] = path[k];
                    path[j].push_back(j);
                }
            }
            dist[k] = 0; 
        }
        else
            break;
    }
    return this->path[finishingPoint.x + finishingPoint.y * point_width];
}
